/*
Usage: First start rviz2, set fixed frame to "map", add PointCloud2, then run the following command:

ros2 run map_manager demo_load_pcl --ros-args -p pcl_file_path:=/ws_618/src/map_manager/maps/scans.pcd \
    -p path_file_path:=/ws_618/src/map_manager/maps/path.csv \
    -p min_pz_gain:=0.3 -p max_pz_gain:=1.8

ros2 run map_manager demo_load_pcl --ros-args -p pcl_file_path:=/home/kuavo/Coding/ws_618/src/map_manager/maps/scans.pcd \
    -p path_file_path:=/home/kuavo/Coding/ws_618/src/map_manager/maps/path.csv \
    -p min_pz_gain:=0.3 -p max_pz_gain:=0.6
*/

#include "rclcpp/rclcpp.hpp"
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <iostream>
#include <sensor_msgs/msg/point_cloud2.h>
#include <pcl_conversions/pcl_conversions.h>

using point_cloud_xyz = pcl::PointCloud<pcl::PointXYZ>;

double min_pz = 1e9, max_pz = -1e9, min_pz_gain = 1.0, max_pz_gain = 1.0;

void update_min_max_pz_from_path_file(const std::string &path_file_path) {
    // Implement CSV file loading logic here if needed
    std::string line;
    std::ifstream file(path_file_path);
    if (!file.is_open()) {
        RCLCPP_ERROR(rclcpp::get_logger("PclTestNode"), "Failed to open path file: %s", path_file_path.c_str());
        return;
    } else {
        RCLCPP_INFO(rclcpp::get_logger("PclTestNode"), "Loading path file from: %s", path_file_path.c_str());
    }

    // Read the first line to find the index of the 'pz' column
    std::getline(file, line);
    int pz_col_index = -1;
    std::istringstream ss(line);
    std::string item;
    while (std::getline(ss, item, ',')) {
        ++pz_col_index;
        if (item == "pz") break;
    }
    if (pz_col_index == -1) {
        RCLCPP_ERROR(rclcpp::get_logger("PclTestNode"), "Column 'pz' not found in path file: %s", path_file_path.c_str());
        return;
    }

    // Read the rest of the file to find min and max pz values
    while (std::getline(file, line)) {
        std::istringstream ss(line);
        double pz = 0.0;
        int col_index = 0;
        while (std::getline(ss, item, ',')) {
            if (col_index == pz_col_index) { // Assuming pz is the third column
                pz = std::stod(item);
                min_pz = std::min(min_pz, pz);
                max_pz = std::max(max_pz, pz);
                break;
            }
            ++col_index;
        }
    }
}

class PclTestNode: public rclcpp::Node {
public:
    PclTestNode(): Node("pcl_test_node") {
        this->declare_parameter<std::string>("pcl_file_path", "");
        this->declare_parameter<std::string>("path_file_path", "");
        this->declare_parameter<double>("min_pz_gain", 1.0);
        this->declare_parameter<double>("max_pz_gain", 1.0);
        this->declare_parameter<bool>("reverse", false);
        pcl_file_path_ = this->get_parameter("pcl_file_path").as_string();
        path_file_path_ = this->get_parameter("path_file_path").as_string();
        min_pz_gain = this->get_parameter("min_pz_gain").as_double();
        max_pz_gain = this->get_parameter("max_pz_gain").as_double();
        bool reverse = this->get_parameter("reverse").as_bool();
        if (pcl_file_path_.empty() || path_file_path_.empty()) {
            RCLCPP_ERROR(this->get_logger(), "Parameter 'pcl_file_path' and 'path_file_path'must be provided! Usage: "
                "ros2 run map_manager demo_load_pcl --ros-args -p pcl_file_path:=/path/to/your/file.pcd path_file_path:=/path/to/your/file.csv");
            rclcpp::shutdown();
            return;
        }

        update_min_max_pz_from_path_file(path_file_path_);
        RCLCPP_INFO(this->get_logger(), "Path file loaded. Min pz: %.2f, Max pz: %.2f", min_pz, max_pz);

        RCLCPP_INFO(this->get_logger(), "Loading PCL file from: %s", pcl_file_path_.c_str());
        cloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("cloud", 10);
        load_pcl_file();
        clip_cloud_and_reverse(reverse);
        publish_cloud();
    }
    
    void load_pcl_file() {
        if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcl_file_path_, *cloud_ptr_) == -1) {
            RCLCPP_ERROR(this->get_logger(), "Failed to load PCD file: %s", pcl_file_path_.c_str());
            rclcpp::shutdown();
            return;
        }
        RCLCPP_INFO(this->get_logger(), "Loaded PCL file with %zu points.", cloud_ptr_->size());
        RCLCPP_INFO(this->get_logger(), "width: %u, height: %u, is_dense: %s",
            cloud_ptr_->width, cloud_ptr_->height, cloud_ptr_->is_dense ? "true" : "false");
        RCLCPP_INFO(this->get_logger(), "First point: x=%.2f, y=%.2f, z=%.2f",
            cloud_ptr_->points[0].x, cloud_ptr_->points[0].y, cloud_ptr_->points[0].z);
        RCLCPP_INFO(this->get_logger(), "Sensor origin (xyz): [%.2f, %.2f, %.2f] / orientation (xyzw): [%.2f, %.2f, %.2f, %.2f]",
            cloud_ptr_->sensor_origin_.x(), cloud_ptr_->sensor_origin_.y(), cloud_ptr_->sensor_origin_.z(),
            cloud_ptr_->sensor_orientation_.x(), cloud_ptr_->sensor_orientation_.y(),
            cloud_ptr_->sensor_orientation_.z(), cloud_ptr_->sensor_orientation_.w());
    }

    void clip_cloud_and_reverse(bool reverse) {
        /* Clip the cloud_ptr_ based on the min and max pz values obtained from the path file. */
        
        RCLCPP_INFO(this->get_logger(), "Clipping point cloud based on path file...");
        auto filtered_cloud = std::make_shared<point_cloud_xyz>();
        filtered_cloud->header = cloud_ptr_->header;
        filtered_cloud->is_dense = true;
        for (auto &point : cloud_ptr_->points) {
            // Clip based on min and max pz with gain
            if (point.z < min_pz - min_pz_gain || point.z > max_pz + max_pz_gain) {
                continue;
            }
            if (reverse) {
                // Reverse the point coordinates
                point.x = -point.x;
                // point.y = -point.y;
                point.z = -point.z;
            }
            filtered_cloud->points.push_back(point);
        }
        filtered_cloud->width = filtered_cloud->points.size();
        filtered_cloud->height = 1;
        filtered_cloud->is_dense = true;
        cloud_ptr_ = filtered_cloud;
        RCLCPP_INFO(this->get_logger(), "Clipping completed. Remaining points: %zu", cloud_ptr_->size());
    }

    void publish_cloud() {
        RCLCPP_INFO(this->get_logger(), "Publishing point cloud...");
        sensor_msgs::msg::PointCloud2 msg;
        pcl::toROSMsg(*cloud_ptr_, msg);
        msg.header.frame_id = "map";
        msg.header.stamp = this->now();
        cloud_pub_->publish(msg);
        RCLCPP_INFO(this->get_logger(), "Point cloud published successfully.");
    }

private:
    std::string pcl_file_path_, path_file_path_;
    point_cloud_xyz::Ptr cloud_ptr_ = std::make_shared<point_cloud_xyz>();
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PclTestNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}